#include "arms_measure.h"


float dtor(float d)
{
    return (3.14159f*d)/180.0f;
}

float rtod(float r)
{
    return (r*180.0f)/(3.14159f);
}

float tri_cos(float d1,float d2,float l1)
{
    if(d1+d2<=l1 || fabs(d1-d2)>=l1)
    {
        return 361;
    }
    else
    {
        return ((pow(d1,2)+pow(d2,2)-pow(l1,2))/(2*d1*d2));
    }
}

float re_dis(float x1,float y1,float x2,float y2)
{
    return pow(pow(x1-x2,2)+pow(y1-y2,2),0.5);
}

void arms_js(float* data_ref,float x1,float y1,float alphas,float l1,float l2,float l3)
{
//    		float x1,y1,ob,ao,ab,alpha6,alpha1,alpha7,alpha4,alpha2,alpha8,oc,alpha5,alpha9,temps,flags=0;
//    x1=x0-l0*cos(dtor(alpha));
//    y1=y0+l0*sin(dtor(alpha));
//    ob=pow(pow(x1,2)+pow(y1,2),0.5);
//    ao=l1;
//    ab=l2;
   
//    alpha1=0;
//    if(x1==0){
//        alpha1=dtor(90);
//    }
//    else if(x1>0){
//        alpha1=atan(fabs(y1)/fabs(x1));
//    }
//    else{
//        alpha1=dtor(180)-atan(fabs(y1)/fabs(x1));
//    }
//    //alpha6=acos((pow(ob,2)+pow(ao,2)-pow(ab,2))/(2*ob*ao));
//    temps=tri_cos(ob,ao,ab);
//    if(temps==361){
//        flags=1;
//    }
//    else{
//        alpha6=acos(temps);
//    }
//    alpha7=alpha1+alpha6;
//    temps=tri_cos(ab,ao,ob);
//    if(temps== 361){
//        flags=1;
//    }
//    else{
//        alpha4=acos(temps);
//    }
//    //alpha4=acos((pow(ab,2)+pow(ao,2)-pow(ob,2))/(2*ab*ao));
//    alpha2=alpha4+alpha7- dtor(180);
//	temps=tri_cos(ab,ob,ao);
//    if (temps==361)
//    {
//        flags=1;
//    }
//    else{
//        alpha5=acos(temps);
//    }
//        //alpha5=acos((pow(ab,2)+pow(ob,2)-pow(ao,2))/(2*ob*ab));
//		oc=pow(pow(x0,2)+pow(y0,2),0.5);
//	temps=tri_cos(ob,l0,oc);
//    if(temps==361){
//        flags=1;
//    }
//    else{
//        alpha9=acos(temps);
//    }
//        //alpha9=acos((pow(ob,2)+pow(l0,2)-pow(oc,2))/(2*ob*l0));
//    data_ref[0]=rtod(alpha7);
//    data_ref[1]=rtod(alpha2);
//    
//    data_ref[1]=180-data_ref[0]+data_ref[1];
//	data_ref[2]=rtod(alpha5)+rtod(alpha9)-90;
//    if(flags==1){
//        data_ref[3]=1;
//    }
//    else{
//        data_ref[3]=0;
//    }
		//if(data_ref[2]>=360){
		//	data_ref[2]-=360;
		//}
    //data_ref[2]=90-(360-data_ref[2]);

    float temps1,x2,y2,x3,y3,x4,y4=0,bd,ad,ab,bc,co,ob,ao,alpha_s,alpha1,alpha2,alpha3,alpha4,alpha5,alpha6,alpha7,kbc,kbc_f,flag=0;
    ab=l1;
    bc=l2;
    co=l3;
    alpha_s=dtor(alphas);
    x2=x1-l1*cos(alpha_s);
    y2=y1-l1*sin(alpha_s);
    ob= re_dis(x2,y2,0,0);
    temps1= tri_cos(ob,co,bc);
    if(temps1==361){
        flag=1;
    }
    else{
        alpha4= acos(temps1);
    }
    ao= re_dis(x1,y1,0,0);
    temps1= tri_cos(ob,ao,ab);
    if(temps1==361){
        flag=1;
    }
    else{
        alpha5= acos(temps1);
    }
    alpha6= atan(y1/x1);
    alpha3=alpha4+alpha5+alpha6;
    temps1= tri_cos(bc,co,ob);
    if(temps1==361){
        flag=1;
    }
    else{
        alpha2= acos(temps1);
    }
    alpha7=dtor(270)-alpha4-alpha5-alpha6;
    x3=0-co*sin(alpha7);
    y3=0-co*cos(alpha7);
    kbc=(y2-y3)/(x2-x3);
    kbc_f=-(1/kbc);
    x4=x2-(y2/kbc_f);
    bd= re_dis(x2,y2,x4,y4);
    ad= re_dis(x1,y1,x4,y4);
    temps1= tri_cos(ab,bd,ad);


    if(temps1==361){
        flag=1;
    }
    else{
        alpha1= dtor(180)-acos(temps1);
    }
    alpha1= rtod(alpha1);
    alpha2= rtod(alpha2);
    alpha3= rtod(alpha3);
    if(alpha1>190){
        alpha1=190;
    }
    else if(alpha1<0){
        alpha1=0;
    }
    if(alpha2>240){
        alpha2=240;
    }
    else if(alpha2<25){
        alpha2=25;
    }
    if(alpha3>230){
        alpha3=230;
    }
    else if(alpha3<0){
        alpha3=0;
    }
    data_ref[0]=alpha1-5;
//		if(data_ref[0]>180){
//			data_ref[0]=180;
//		}
    data_ref[1]=alpha2;
    data_ref[2]=alpha3;
    data_ref[3]=flag;

}

